#include "SensImg.h"
#include <iostream>
#include <cstdlib>
#include <cstring>

int show_data(Sensor::tsens_data data) {
  std::cout << "x:" << data.x << " ";
  std::cout << "y:" << data.y << " ";
  std::cout << "z:" << data.z << " ";
  std::cout << "keys:" << data.keys << " ";
  std::cout << "timestamp:" << data.timestamp << " ";
  std::cout << "devid:" << data.devid << " ";
  std::cout << "\n";
  return 0;
}

void receive_for_secs(int secs) {
  Sensor::SensorReceiver sens_receiver(show_data);  
  std::cout << "Receiving with callbacks for " << secs << " secs \n";
  sleep(secs);
}

void poll_for_secs(int secs) {
  Sensor::SensorPoller sens_poller;
  std::cout << "Polling for " << secs << " secs \n";
  struct timespec sleep_time;
  sleep_time.tv_sec = 0;
  sleep_time.tv_nsec = 100*1000*1000;
  secs *= 10;
  while(secs--) {
    show_data(sens_poller.poll());
    if (nanosleep(&sleep_time, NULL) != 0) {
      std::cout << "sleeping problems\n";
      break;
    }
  }  
}

int main(int argc, char *argv[]) {
  std::cout << "C++ wrapper for sens_receiver and img_sender\n";
  std::cout << "  -c n receive with callbacks for n seconds\n";
  std::cout << "  -p n receive with polling (10 per sec) for n seconds\n";
  if (argc == 1) {
    receive_for_secs(60);
  } else if (argc == 2) {
    return -1;
  } else if (argc == 3) {
    if (strcmp(argv[1], "-c") == 0) {
      receive_for_secs(atoi(argv[2]));
    } else if (strcmp(argv[1], "-p") == 0) {
      poll_for_secs(atoi(argv[2]));
    }
  }
  return 0;
}
